﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

using DXBase;
using System.Runtime.InteropServices;

namespace DXBase.Structs
{
	/// <summary>
	/// This class is used to represent affine transformation of 3D space.
	/// However it is prone to cumulative numerical error, hence it is usually 
	/// only used to pass the final transform to DirectX
	/// </summary>
	[StructLayout(LayoutKind.Sequential)]
	public struct Matrix4x4F
	{
		// this struct has the same layout as DXBase.float4x4
		public float m00, m01, m02, m03;
		public float m10, m11, m12, m13;
		public float m20, m21, m22, m23;
		public float m30, m31, m32, m33;

		public Matrix4x4F(
			float i11, float i12, float i13, float i14,
			float i21, float i22, float i23, float i24,
			float i31, float i32, float i33, float i34,
			float i41, float i42, float i43, float i44
		)
		{
			m00 = i11; m01 = i12; m02 = i13; m03 = i14;
			m10 = i21; m11 = i22; m12 = i23; m13 = i24;
			m20 = i31; m21 = i32; m22 = i33; m23 = i34;
			m30 = i41; m31 = i42; m32 = i43; m33 = i44;
		}
		public Matrix4x4F(Vector3F col0, Vector3F col1, Vector3F col2)
		{
			m00 = col0.X; m01 = col1.X; m02 = col2.X; m03 = 0;
			m10 = col0.Y; m11 = col1.Y; m12 = col2.Y; m13 = 0;
			m20 = col0.Z; m21 = col1.Z; m22 = col2.Z; m23 = 0;
			m30 = 0; m31 = 0; m32 = 0; m33 = 1;
		}
		public Matrix4x4F(Vector3F col0, Vector3F col1, Vector3F col2, Vector3F col3)
		{
			m00 = col0.X; m01 = col1.X; m02 = col2.X; m03 = col3.X;
			m10 = col0.Y; m11 = col1.Y; m12 = col2.Y; m13 = col3.Y;
			m20 = col0.Z; m21 = col1.Z; m22 = col2.Z; m23 = col3.Z;
			m30 = 0; m31 = 0; m32 = 0; m33 = 1;
		}

		public unsafe static implicit operator Matrix4x4F(float4x4 v) 
		{
			Matrix4x4F dst;
			*&dst = *(Matrix4x4F*)&v;
			return dst;
		}
		public unsafe static implicit operator float4x4(Matrix4x4F v) 
		{
			float4x4 dst;
			*&dst = *(float4x4*)&v;
			return dst;
		}

		unsafe public float this[int row, int col]
		{
			get
			{
				if (row < 0 || col < 0 || row >= 4 || col >= 4)
					throw new ArgumentOutOfRangeException();
				fixed (float* p = &m00)
					return p[row * 4 + col];
			}
			set
			{
				if (row < 0 || col < 0 || row >= 4 || col >= 4)
					throw new ArgumentOutOfRangeException();
				fixed (float* p = &m00)
					p[row * 4 + col] = value;
			}
		}

		public static Matrix4x4F Identity()
		{
			return new Matrix4x4F(
				1, 0, 0, 0,
				0, 1, 0, 0,
				0, 0, 1, 0,
				0, 0, 0, 1
			);
		}
		public static Matrix4x4F Translation(float x, float y, float z)
		{
			return new Matrix4x4F(
				1, 0, 0, x,
				0, 1, 0, y,
				0, 0, 1, z,
				0, 0, 0, 1
			);
		}
		public static Matrix4x4F Translation(Vector3F v)
		{
			return new Matrix4x4F(
				1, 0, 0, v.X,
				0, 1, 0, v.Y,
				0, 0, 1, v.Z,
				0, 0, 0, 1
			);
		}
		public static Matrix4x4F Scale(float x, float y, float z)
		{
			return new Matrix4x4F(
				x, 0, 0, 0,
				0, y, 0, 0,
				0, 0, z, 0,
				0, 0, 0, 1
			);
		}
		public static Matrix4x4F Scale(Vector3F v)
		{
			return new Matrix4x4F(
				v.X, 0, 0, 0,
				0, v.Y, 0, 0,
				0, 0, v.Z, 0,
				0, 0, 0, 1
			);
		}
		public static Matrix4x4F Rotation(float axisX, float axisY, float axisZ, float degrees)
		{
			return QuaternionF.Rotation(axisX, axisY, axisZ, degrees).ToMatrix();
		}
		public static Matrix4x4F Rotation(Vector3F axis, float degrees)
		{
			return QuaternionF.Rotation(axis, degrees).ToMatrix();
		}

		public Matrix4x4F Transpose()
		{
			return new Matrix4x4F(
				m00, m10, m20, m30,
				m01, m11, m21, m31,
				m02, m12, m22, m32,
				m03, m13, m23, m33
				);
		}
		public Matrix4x4F Invert()
		{
			return FromSquareMatrix(ToSquareMatrix().Invert());
		}

		unsafe public SquareMatrixF ToSquareMatrix()
		{
			var result = new SquareMatrixF(4);
			fixed (float* pDst = &result.GetDataArray()[0])
			fixed (float* pSrc = &m00)
			{
				for (int i = 0; i < 16; i++)
					pDst[i] = pSrc[i];
			}
			return result;
		}
		unsafe public static Matrix4x4F FromSquareMatrix(SquareMatrixF m)
		{
			if (m.Size != 4)
				throw new ArgumentOutOfRangeException();
			Matrix4x4F result = new Matrix4x4F();
			fixed (float* pSrc = &m.GetDataArray()[0])
			{
				float* pDst = &result.m00;
				for (int i = 0; i < 16; i++)
					pDst[i] = pSrc[i];
			}
			return result;
		}

		public unsafe static Matrix4x4F operator *(Matrix4x4F m1, Matrix4x4F m2)
		{
			Matrix4x4F res;
			float* pRes = (float*)&res;
			float* pM1 = (float*)&m1;
			float* pM2 = (float*)&m2;
			Func<int, int, int> index = (row, col) => row * 4 + col;
			for (int i = 0; i < 4; i++)
				for (int j = 0; j < 4; j++)
				{
					int pos = index(i, j);
					pRes[pos] = 0;
					for (int k = 0; k < 4; k++)
						pRes[pos] += pM1[index(i, k)] * pM2[index(k, j)];
				}
			return res;
		}
		public static Vector4F operator *(Matrix4x4F m, Vector4F p)
		{
			return new Vector4F(
				m.m00 * p.X + m.m01 * p.Y + m.m02 * p.Z + m.m03 * p.W,
				m.m10 * p.X + m.m11 * p.Y + m.m12 * p.Z + m.m13 * p.W,
				m.m20 * p.X + m.m21 * p.Y + m.m22 * p.Z + m.m23 * p.W,
				m.m30 * p.X + m.m31 * p.Y + m.m32 * p.Z + m.m33 * p.W
				);
		}

		public Vector3F ConvertPoint(Vector3F p) 
		{
			return new Vector3F(
				m00 * p.X + m01 * p.Y + m02 * p.Z + m03,
				m10 * p.X + m11 * p.Y + m12 * p.Z + m13,
				m20 * p.X + m21 * p.Y + m22 * p.Z + m23
				);
		}
		public Vector3F ConvertVector(Vector3F v) 
		{
			return new Vector3F(
				m00 * v.X + m01 * v.Y + m02 * v.Z,
				m10 * v.X + m11 * v.Y + m12 * v.Z,
				m20 * v.X + m21 * v.Y + m22 * v.Z
				);
		}

		public override string ToString()
		{
			var sb = new StringBuilder();
			for (int i = 0; i < 4; i++)
			{
				if (i == 0) sb.Append('[');
				else sb.AppendLine().Append(' ');
				for (int j = 0; j < 4; j++)
				{
					if (j > 0) sb.Append(", ");
					sb.Append(this[i, j]);
				}
			}
			sb.Append(']');
			return sb.ToString();
		}
	}
}
